using System;
using System.Data;
using System.Data.Common;
using System.Collections.Generic;
using System.Drawing;
using System.Globalization;
using System.Windows.Forms;
using Diversity.Maths;

namespace Diversity.Robotics.Navigation.GridSlamApp
{
    /// <summary>
    /// Far too many functions here that should move away. It is a playground.
    /// </summary>
    public partial class Form1 : Form
    {
        #region Private fields an Initialisation
       // private ProbabilityUtils ProbabilityUtils;

        // new headless grid slam
        private GridSlam _gridSlam;

        // Two alternative cone models
        private ConeModel1Gaussian _coneModel;

        public ConeModel1Gaussian ConeModel
        {
            get { return _coneModel; }
            set { _coneModel = value; }
        }
        private ConeModel4Gaussians _coneModel4Gaussians;
        private BeamModel _beamModel;

        public BeamModel BeamModel
        {
            get { return _beamModel; }
            set { _beamModel = value; }
        }
        private Robot _robot;
        private RobotFromLogFile _robotFromLogFile;

        public RobotFromLogFile LogFileRobot
        {
            get { return _robotFromLogFile; }
            set { _robotFromLogFile = value; }
        }

        VelocityMotionModel _motionModel;
        private OdometryMotionModel _odometryMotionModel;
        private Pose2D _odometryModelState;
        private Pose2D[] _odometryModelStates;

        // For the vector field histogram
        private PolarObstacleDensityMap _polarObstacleDensityMap;

        // TODO: depreciate all this
        private VelocityMotionParticleFilter _pf;
        private List<DifferentialDriveStateSample> _samples = null;


        public Form1()
        {
            InitializeComponent();
            
           // ProbabilityUtils = new ProbabilityUtils();
          
            LoadDefaultMap();
            Image i2 = new Bitmap(pictureBox2.Width, pictureBox2.Height);
            pictureBox2.Image = i2;

            // create the motion model
            buttonVelocityMotionModelSave_Click(null, new EventArgs());

            // Prepare Sonar Model
            buttonConeModelSave_Click(null, new EventArgs());

            buttonBeamModelSave_Click(null, new EventArgs());

            // prepare more detailed cone model
            buttonConeModel4DSave_Click(null, new EventArgs());
           
            

            buttonVFHSave_Click(null, new EventArgs());

            // create the robot
            buttonRobotSave_Click(null,new EventArgs());
            textBoxSensorsNum_Leave(null, new EventArgs());
            buttonRobotTestContraints_Click(null,new EventArgs());
            

            comboBoxConeModel.SelectedIndex = 0;
            comboBoxNavigator.SelectedIndex = 0;

            buttonConeModel4DTestHit_Click(null, new EventArgs());

            pictureBoxSLAMEffectiveWeight.Image =
                new Bitmap(pictureBoxSLAMEffectiveWeight.Width, pictureBoxSLAMEffectiveWeight.Height);
            pictureBoxSLAMBest.Image = new Bitmap(pictureBoxSLAMBest.Width, pictureBoxSLAMBest.Height);
            pictureBoxSLAMWorst.Image = new Bitmap(pictureBoxSLAMWorst.Width, pictureBoxSLAMWorst.Height);
            pictureBoxSLAMReal.Image = new Bitmap(pictureBoxSLAMReal.Width, pictureBoxSLAMReal.Height);
        }
        #endregion

        #region Motion Model


        /// <summary>
        /// Saves the parameters of the motion model
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void buttonVelocityMotionModelSave_Click(object sender, EventArgs e)
        {
            _motionModel = new VelocityMotionModel(
                double.Parse(textBoxVerr1.Text, CultureInfo.InvariantCulture),
                double.Parse(textBoxVerr2.Text, CultureInfo.InvariantCulture),
                double.Parse(textBoxVerr3.Text, CultureInfo.InvariantCulture),
                double.Parse(textBoxRerr1.Text, CultureInfo.InvariantCulture),
                double.Parse(textBoxRerr2.Text, CultureInfo.InvariantCulture),
                double.Parse(textBoxRerr3.Text, CultureInfo.InvariantCulture));
            _pf = new VelocityMotionParticleFilter(_motionModel, int.Parse(textBoxParticles.Text));
           
            _samples = null;

            Image i2 = new Bitmap(pictureBox2.Width, pictureBox2.Height);
            pictureBox2.Image = i2;
            pictureBox2.Refresh();
        }

        /// <summary>
        /// Velocity Motion Particle Filter
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void button1_Click(object sender, EventArgs e)
        {
            // TODO ALL THIS NEEDS CLEANING UP TO USE Diversity.Maths.ProbabilityUtils.cs instead.

            Velocity vel = new Velocity(
                double.Parse(textBoxV.Text, CultureInfo.InvariantCulture),
                double.Parse(textBoxW.Text, CultureInfo.InvariantCulture));


            if (_pf == null)
            {
                _pf = new VelocityMotionParticleFilter(_motionModel, int.Parse(textBoxParticles.Text));
            }

            if (_samples == null || _samples.Count == 0)
            {
                Pose2D state = new Pose2D(
                    double.Parse(textBoxX.Text, CultureInfo.InvariantCulture),
                    double.Parse(textBoxY.Text, CultureInfo.InvariantCulture),
                    double.Parse(textBoxTheta.Text, CultureInfo.InvariantCulture));
                _samples = _pf.CreateStartSamples(state);
                DrawSamples(_samples, pictureBox2.Image);
            }

            DateTime t0 = DateTime.Now;
            double totalProbability;
            _samples =
                _pf.GetSamples(_samples, vel,
                              new TimeSpan(0, 0, 0, 0, (int)Math.Round(double.Parse(textBoxSeconds.Text, CultureInfo.InvariantCulture) * 1000)),
                              out totalProbability);
            DateTime t1 = DateTime.Now;
         
            // normalize sample set to total prob == 1
            DateTime t2 = DateTime.Now;
            _pf.Normalize(_samples, totalProbability);
            DateTime t3 = DateTime.Now;
            if (checkBoxMotionModelResample.Checked)
            {
                _samples = _pf.ReSample(_samples, out totalProbability);
                DateTime t4 = DateTime.Now;
                _pf.Normalize(_samples, totalProbability);
                DateTime t5 = DateTime.Now;

                double timeToSample = TimeSpan.FromTicks(t1.Ticks - t0.Ticks).TotalSeconds;
                double timeToNormalizeSamples = TimeSpan.FromTicks(t3.Ticks - t2.Ticks).TotalSeconds;
                double timeToReSample = TimeSpan.FromTicks(t4.Ticks - t3.Ticks).TotalSeconds;
                double timeToNormalizeReSamples = TimeSpan.FromTicks(t5.Ticks - t4.Ticks).TotalSeconds;
                double totalTime = timeToNormalizeReSamples + timeToNormalizeSamples + timeToReSample + timeToSample;
                Console.Out.WriteLine("Velocity Particle Filter processed " + _samples.Count + " particles in " +
                                      totalTime);
                Console.Out.WriteLine("Sampling: " + timeToSample + " normalization: + " + timeToNormalizeSamples);
                Console.Out.WriteLine("Re-Sampling: " + timeToReSample + " normalization: + " + timeToNormalizeReSamples);
               
            }

            DrawSamples(_samples, pictureBox2.Image);

            // draw perfect sample

            Graphics g = Graphics.FromImage(pictureBox2.Image);
            Point perfectPoint = _pf._perfectState.ToPoint();
            g.DrawLine(Pens.Red, perfectPoint.X, perfectPoint.Y,perfectPoint.X+1,perfectPoint.Y+1);
            g.Dispose();
            pictureBox2.Refresh();
        }


        private void DrawSamples(List<DifferentialDriveStateSample> samples, Image i)
        {
            // labelDebug.Text += "Iterate. ";
            //  Bitmap b = (Bitmap)i;

            List<long> doneAlready = new List<long>();
            TimeSpan refreshInterval = new TimeSpan(0, 0, 0, 0, 500);
            DateTime lastTime = DateTime.Now;

            Graphics g = Graphics.FromImage(i);
            foreach (DifferentialDriveStateSample sample in samples)
            {
                long key = (long)Math.Round((sample.State.X * i.Width) + sample.State.Y);
                if (!doneAlready.Contains(key))
                {
                    doneAlready.Add(key);
                    //if (b.GetPixel((int)Math.Round(sample.State.X),(int)Math.Round(sample.State.Y)).R > 200)
                    // {
                    // b.SetPixel((int)Math.Floor(sample.State.X), (int)Math.Floor(sample.State.Y), Color.Blue);
                    g.DrawLine(Pens.Black, (float)sample.State.X, (float)sample.State.Y, (float)sample.State.X + 1,  (float)sample.State.Y + 1);
                    // }
                    //labelDebug.Text += ".";
                    if (DateTime.Now.Ticks - lastTime.Ticks > refreshInterval.Ticks)
                    {
                        lastTime = DateTime.Now;
                        g.Save();
                        pictureBox2.Refresh();
                    }

                }
            }
            g.Save();
            g.Dispose();
            pictureBox2.Refresh();
        }

        #endregion

        #region Draw Motion Model Re-sampling
        /*
        private static void DrawProbabilityDistribution(List<DifferentialDriveStateSample> samples, Image img)
        {
            // bad: step through the entire list to find the average
            double centerX = 0;
            double centerY = 0;
            double centerTheta = 0;
            double maxProb = 0;
            double minProb = double.MaxValue;
            double minX = double.MaxValue;
            double maxX = 0;
            double minY = double.MaxValue;
            double maxY = 0;
            double minTheta = double.MaxValue;
            double maxTheta = double.MinValue;
            double totalProb = 0;
            foreach (DifferentialDriveStateSample sample in samples)
            {
                centerX += sample.State.X;
                centerY += sample.State.Y;
                centerTheta += sample.State.Theta;
                totalProb += sample.Weight;
                if (sample.Weight > maxProb)
                    maxProb = sample.Weight;
                if (sample.Weight < minProb)
                    minProb = sample.Weight;
                if (sample.State.X > maxX)
                    maxX = sample.State.X;
                if (sample.State.X < minX)
                    minX = sample.State.X;
                if (sample.State.Y > maxY)
                    maxY = sample.State.Y;
                if (sample.State.Y < minY)
                    minY = sample.State.Y;
                if (sample.State.Theta > maxTheta)
                    maxTheta = sample.State.Theta;
                if (sample.State.Theta < minTheta)
                    minTheta = sample.State.Theta;
            }
            centerX = centerX / samples.Count;
            centerY = centerY / samples.Count;
            centerTheta = centerTheta / samples.Count;
            Console.Out.WriteLine("Average: (" + centerX + "," + centerY + "," + centerTheta + ") " +
                " Total Prob: " + totalProb + " Min: " + minProb + " Max: " + maxProb + " Av: " + totalProb / samples.Count);
            Console.Out.WriteLine("Min: (" + minX + "," + minY + "," + minTheta + ") Max: (" + maxX + "," + maxY + "," + maxTheta + ")");
            // step through the entire list again, working out divergances
            double[] xDifferences = new double[samples.Count];
            double[] yDifferences = new double[samples.Count];
            double[] tDifferences = new double[samples.Count];
            double[] totalDifference = new double[samples.Count];
            // make frequency bins
            int numBins = 15;
            int[] xBins = new int[numBins];
            int[] yBins = new int[numBins];
            int[] tBins = new int[numBins];
            int[] pBins = new int[numBins];
            double xBinSize = (maxX - minX) / numBins;
            double yBinSize = (maxY - minY) / numBins;
            double tBinSize = (maxTheta - minTheta) / numBins;
            double pBinSize = (maxProb - minProb) / numBins;
            int maxXBin = 0;
            int maxYBin = 0;
            int maxTBin = 0;
            int maxPBin = 0;
            if (xBinSize > 0 && yBinSize > 0 && tBinSize > 0)
            {
                for (int i = 0; i < samples.Count; i++)
                {
                    xDifferences[i] = samples[i].State.X - centerX;
                    yDifferences[i] = samples[i].State.Y - centerY;
                    tDifferences[i] = samples[i].State.Theta - centerTheta;

                    // chuck into bins
                    int xBinIndex = Math.Min(numBins - 1, Math.Max(0, (int)Math.Round((samples[i].State.X - minX) / xBinSize)));
                    int yBinIndex = Math.Min(numBins - 1, Math.Max(0, (int)Math.Round((samples[i].State.Y - minY) / yBinSize)));
                    int tBinIndex = Math.Min(numBins - 1, Math.Max(0, (int)Math.Round((samples[i].State.Theta - minTheta) / tBinSize)));
                    int pBinIndex = Math.Min(numBins - 1, Math.Max(0, (int)Math.Round((samples[i].Weight) / pBinSize)));
                    xBins[xBinIndex]++;
                    yBins[yBinIndex]++;
                    tBins[tBinIndex]++;
                    pBins[pBinIndex]++;
                    if (xBins[xBinIndex] > maxXBin)
                        maxXBin = xBins[xBinIndex];
                    if (yBins[yBinIndex] > maxYBin)
                        maxYBin = yBins[yBinIndex];
                    if (tBins[tBinIndex] > maxTBin)
                        maxTBin = tBins[tBinIndex];
                    if (pBins[pBinIndex] > maxPBin)
                        maxPBin = pBins[pBinIndex];
                }
                // draw the bins
                Graphics g = Graphics.FromImage(img);
                List<PointF> pointsX = new List<PointF>();
                List<PointF> pointsY = new List<PointF>();
                List<PointF> pointsT = new List<PointF>();
                List<PointF> pointsP = new List<PointF>();
                for (int i = 0; i < numBins; i++)
                {
                    pointsX.Add(new PointF((float)((i * img.Width * 1.0) / numBins), img.Height - (float)((xBins[i] * 1.0 * (img.Height - 10)) / maxXBin)));
                    pointsY.Add(new PointF((float)((i * img.Width * 1.0) / numBins), img.Height - (float)((yBins[i] * 1.0 * (img.Height - 10)) / maxYBin)));
                    pointsT.Add(new PointF((float)((i * img.Width * 1.0) / numBins), img.Height - (float)((tBins[i] * 1.0 * (img.Height - 10)) / maxTBin)));
                    pointsP.Add(new PointF((float)((i * img.Width * 1.0) / numBins), img.Height - (float)((pBins[i] * 1.0 * (img.Height - 10)) / maxPBin)));

                }
                g.DrawCurve(Pens.Red, pointsX.ToArray());
                g.DrawCurve(Pens.Blue, pointsY.ToArray());
                g.DrawCurve(Pens.Green, pointsT.ToArray());
                g.DrawCurve(Pens.Black, pointsP.ToArray());
            }


            // 
        }
         */ 
        #endregion

        #region Little Utilities
       

        public Rectangle MergeRects(Rectangle r1, Rectangle r2)
        {
            int left = Math.Min(r1.Left,r2.Left);
            int top = Math.Min(r1.Top,r2.Top);
            int width = Math.Max(r1.Right,r2.Right) - left;
            int height = Math.Max(r1.Bottom,r2.Bottom) - top;
            return new Rectangle(left, top, width, height);
        }

        #endregion

        #region SLAM

        /// <summary>
        /// Invoke the SLAM algorythmn
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void buttonSLAMGo_Click(object sender, EventArgs e)
        {
            //pictureBoxSLAMAllParticles.Image = (Image)new Bitmap(pictureBoxSLAMAllParticles.Width, pictureBoxSLAMAllParticles.Height);

            // clear the images
            pictureBoxSLAMEffectiveWeight.Image =
              new Bitmap(pictureBoxSLAMEffectiveWeight.Width, pictureBoxSLAMEffectiveWeight.Height);
           // pictureBoxSLAMAllParticles.Image =
           //     new Bitmap(pictureBoxSLAMAllParticles.Width, pictureBoxSLAMAllParticles.Height);
            pictureBoxSLAMBest.Image = new Bitmap(pictureBoxSLAMBest.Width, pictureBoxSLAMBest.Height);
            pictureBoxSLAMWorst.Image = new Bitmap(pictureBoxSLAMWorst.Width, pictureBoxSLAMWorst.Height);
            pictureBoxSLAMReal.Image = new Bitmap(pictureBoxSLAMReal.Width, pictureBoxSLAMReal.Height);

            double effectiveWeightPercent = double.Parse(textBoxSLAMEffectiveWeightPercent.Text, CultureInfo.InvariantCulture);
            double informationReduction = double.Parse(textBoxSLAMInformationReduction.Text, CultureInfo.InvariantCulture);
            int numParticles = int.Parse(textBoxSLAMParticles.Text);
            int numSteps = int.Parse(textBoxSLAMSteps.Text);
            double[] effectiveWeightHistory = new double[numSteps];
            double[] resamplingHistory = new double[numSteps];
            double[] rmsErrorHistory = new double[numSteps];

            Map globalMap = new Map(pictureBoxGlobalMap.Width, pictureBoxGlobalMap.Height, 0.6, 10);
            // load the global map from the global localization image
            globalMap.LoadImage(pictureBoxGlobalMap.Image);


            // create a robot
            if (checkBoxUseLogFileRobot.Checked)
            {
                // hacked log file robot
               // _robot = new RobotFromLogFile(_robot.State, 10, 10, _robot.GlobalMap, textBoxLogFileName.Text);
                if (_robotFromLogFile != null) {
                _robot = _robotFromLogFile;
            }
                _robot.MotionModel = _motionModel;
            }
            else
            {
                // normal
                buttonRobotSave_Click(null, new EventArgs());
                _robot.GlobalMap = globalMap;
                _robot.LocalMap = new Map(globalMap.Width, globalMap.Height, 0.6, 10);
                Pose2D robotState = new Pose2D(110, 110, 0);
                _robot.Pose2DCurrent = robotState;
                _robot.Velocity = new Velocity(_robot.MaxSpeed, 0);
            }
            
            DistanceReading[] globalMapReadings;

            #region Create Initial Particle Set
            Pose2D[] states = new Pose2D[numParticles];
            Map[] maps = new Map[numParticles];
            double[] weights = new double[numParticles];
            double[] aggregateWeights = new double[numParticles];

            for (int i = 0; i < numParticles;i++ )
            {
                states[i] = (Pose2D)_robot.Pose2DCurrent.Clone();
                maps[i] = new Map(_robot.GlobalMap.Width, _robot.GlobalMap.Height, 0.6, 10);
                weights[i] = 1.0/numParticles;
            }
            Console.Out.WriteLine("Created states and maps");
            #endregion
 //DateTime lastDraw = DateTime.Now;
            for (int step = 0; step < numSteps; step++)
            {
               
                // Let the Robot choose a velocity that is close
                // to a desired velocity. This is done via the INavigate interface
                _robot.Move(_robot.Velocity);
                
                // Stop in case of failure to find a good direction.
                if (_robot.Velocity.Translation == 0 && _robot.Velocity.Rotation == 0)
                {
                    Console.Out.WriteLine("No translation, so stopping");
                    break;
                }
               
                // Get some distance readings from the Global Map (real world)
                globalMapReadings = _robot.GetDistanceReadings(_robot.GlobalMap, _robot.Pose2DCurrent, 0);
                _robot.LocalMap.AddReadings(_robot.Pose2DCurrent, globalMapReadings);
                _robot.LocalMap.ClearRobotPosition(_robot.Pose2DCurrent);

                // take a copy of the weight before messing with them
                double[] mapWeights = new double[weights.Length];

                // Loop around all the particles, moving them and calculating their likelyhood
                for (int i = 0; i < numParticles; i++)
                {
                   
                    Pose2D oldState = (Pose2D)states[i].Clone();
                    bool isAllowedByMap = false;

                    // Get Sample applies the Motion model with added noise
                    int mapFailCount = 0;
                    while (!isAllowedByMap && mapFailCount<10)
                    {
                        mapFailCount++;
                        states[i] = _robot.MotionModel.GetSample(oldState);
                        isAllowedByMap = (checkBoxSLAMConditionSamplesOnMap.Checked == true) ? _robot.IsValidState(maps[i], states[i]) : true;
                    } // this might allow a dead particle through, but it should get filtered out.
                    if (mapFailCount == 10)
                    {
                        Console.Out.WriteLine("Bad move");
                    }

                    // quick cheat test.
                   // if (i == 0)
                   // {
                    //    states[i] = (State)_robot.State.Clone();
                   // }

                    // Before updating the particle's local map, get a scan
                    DistanceReading[] stateReadings = _robot.GetDistanceReadings(maps[i], states[i], 0);

                    // Using the robot's beam model, calculate the correspondance between the real
                    // readings and the particle's scan
                    double probabilityOfScan = _robot.CompareDistanceReadings(stateReadings, globalMapReadings);

                  //  if (i<2)
                   // {
                    //    Console.Out.WriteLine("Prob of " + i + " is " + probabilityOfScan);
                   // }

                    // Weaken this information
                   // probabilityOfScan = Math.Pow(probabilityOfScan, informationReduction);
                    mapWeights[i] = probabilityOfScan;

                    if (probabilityOfScan == 0)
                    {
                        Console.Out.WriteLine("Zero prob scan");
                    }
                  
                }
// prepare the weights for resampling by normalizing to a total prob of one.
                    mapWeights = ProbabilityUtils.Normalize(mapWeights, Normalization.MaxToOne);
                // get a new the effective weight
                effectiveWeightHistory[step] = ProbabilityUtils.GetEffectiveSampleSize(mapWeights);

                // do the information reduction
                for (int i = 0; i < mapWeights.Length; i++)
                {
                    mapWeights[i] = Math.Pow(mapWeights[i], informationReduction);
                }

                // normalize again!
                mapWeights = ProbabilityUtils.Normalize(mapWeights, Normalization.IntegralToOne);
                    

                // TODO use baysein method to update the weights after resampling
                rmsErrorHistory[step] = ProbabilityUtils.GetRMSError(states, _robot.Pose2DCurrent);

                // A generic low variance resampler, that works with a normalized array of weights.
                // The indexes can be used to determine which particles are needed,
                // therefore which maps need updating.
                int[] indexes = ProbabilityUtils.ReSample(mapWeights);

                // now step through the particles an update the weights using baysian approach
                weights = mapWeights;// ProbabilityUtils.BayesUpdate(weights, mapWeights);

               
 
                // draw these normalized weights before doing the resampling
                pictureBoxSLAMWeights.Image = (Image)new Bitmap(pictureBoxSLAMWeights.Width, pictureBoxSLAMWeights.Height);
                pictureBoxSLAMWeights.Image = ProbabilityUtils.DrawSortedDistribution(pictureBoxSLAMWeights.Image, weights, Normalization.None, Color.Blue, true);
                //  pictureBoxSLAMWeights.Refresh();

                if (checkBoxSLAMNoResampling.Checked || (checkBoxSLAMEffectiveWeightEnable.Checked && effectiveWeightHistory[step] > (effectiveWeightPercent)))
                    {
                        // no resampling, so apply the readings to each map
                        for (int i = 0; i < maps.Length; i++)
                        {
                            maps[i].AddReadings(states[i], globalMapReadings);
                        }
                        resamplingHistory[step] = 0;
                    }
                    else
                    {
                       
                        #region Create resampled sets

                        List<int> AllreadyCopied = new List<int>();
                        Pose2D[] newStates = new Pose2D[indexes.Length];
                        Map[] newMaps = new Map[indexes.Length];
                        double[] newAggregates = new double[indexes.Length];
                        double[] newWeights = new double[indexes.Length];
                        int o = 0;
                        foreach (int eee in indexes)
                        {
                            if (AllreadyCopied.Contains(eee))
                            {
                                // alas this particle needs cloning.
                                newStates[o] = (Pose2D) states[eee].Clone();
                                newMaps[o] = (Map) maps[eee].Clone();
                                newWeights[o] = weights[eee];
                                newAggregates[o] = aggregateWeights[eee];

                                // keeps a track of lost particles (i.e. duplicates)
                                resamplingHistory[step]++;
                            }
                            else
                            {
                                // assignment is much faster than cloning
                                AllreadyCopied.Add(eee);
                                newStates[o] = states[eee];
                                maps[eee].AddReadings(states[eee], globalMapReadings);
                                maps[eee].ClearRobotPosition(states[eee]);
                                newMaps[o] = maps[eee];
                                newWeights[o] = weights[eee];
                                newAggregates[o] = aggregateWeights[eee];
                            }
                            o++;
                        }
                        resamplingHistory[step] = resamplingHistory[step]/numParticles;
                        states = newStates;
                        maps = newMaps;
                        weights = newWeights;
                        aggregateWeights = newAggregates;

                        #endregion
                    }

                for(int i=0; i< weights.Length; i++)
                {
                    aggregateWeights[i] += weights[i];
                }
                /*
                if(aggregateWeights[0] == 0)
                {
                    // probably first time round
                    aggregateWeights = (double[])weights.Clone();

                } else
                {
                    aggregateWeights = ProbabilityUtils.BayesUpdate(aggregateWeights, weights);
                }
                  */ 
                #region Unused path drawing
                    // prob = null;
                    /*
                    if (points.Length > 1)
                    {
                        Point[] scaledPoints = new Point[points.Length];
                        for (int pp = 0; pp < points.Length; pp++ )
                        {
                            scaledPoints[pp] = new Point((int)Math.Round(points[pp].X / _robot.GlobalMap.Scale), (int)Math.Round(points[pp].Y / _robot.GlobalMap.Scale));
                        }
                       // Console.Out.WriteLine("Drawing " + scaledPoints.Length);
                        Graphics g = Graphics.FromImage(pictureBoxSLAMReal.Image);
                        g.DrawLines(Pens.Red, scaledPoints);
                        g.Save();
                        g.Dispose();
                    } else
                    {
                        Console.Out.WriteLine("no points");
                    }
                     * */
                    #endregion

                #region Calculate max min and draw results


                    if ( true) //step ==0 || step == numSteps-1 || TimeSpan.FromTicks(DateTime.Now.Ticks - lastDraw.Ticks).Milliseconds > 250)
                    {
                       // lastDraw = DateTime.Now;
                if (double.IsNaN(aggregateWeights[0]))
                {
                    Console.Out.WriteLine("Problem");
                }
                else
                {
                    double maxAggregate = double.MinValue;
                    double minAggregate = double.MaxValue;
                    int indexOfMax = 0;
                    int indexOfWorst = 0;
                    for (int i = 0; i < numParticles; i++)
                    {
                        if (aggregateWeights[i] > maxAggregate)
                        {
                            maxAggregate = aggregateWeights[i];
                            indexOfMax = i;
                        }
                        if (aggregateWeights[i] < minAggregate)
                        {
                            minAggregate = aggregateWeights[i];
                            indexOfWorst = i;
                        }
                    }
                    // draw re-sampled weights
                    pictureBoxSLAMWeights.Image =
                        ProbabilityUtils.DrawSortedDistribution(pictureBoxSLAMWeights.Image, weights, Normalization.None,
                                                          Color.Red, false);
                    pictureBoxSLAMWeights.Refresh();
                    // double[] normalizedAggregateWeights = ProbabilityUtils.Normalize(aggregateWeights, Normalization.MaxToOne);
                    pictureBoxSLAMAggregateWeights.Image =
                        (Image) new Bitmap(pictureBoxSLAMAggregateWeights.Width, pictureBoxSLAMAggregateWeights.Height);
                    pictureBoxSLAMAggregateWeights.Image =
                        ProbabilityUtils.DrawSortedDistribution(pictureBoxSLAMAggregateWeights.Image, aggregateWeights,
                                                          Normalization.None, Color.Blue, true);
                    pictureBoxSLAMAggregateWeights.Refresh();
                    
                    // Draw the perfect map
                    pictureBoxSLAMReal.Image =
                        ProbabilityUtils.DrawLogOddsDistribution(pictureBoxSLAMReal.Image, _robot.LocalMap.Occupancy);
                    pictureBoxSLAMReal.Refresh();

                    // Draw the best map
                   //pictureBoxSLAMBest.Image =
                        Bitmap tmpBest = (Bitmap)  ProbabilityUtils.DrawLogOddsDistribution(pictureBoxSLAMBest.Image, maps[indexOfMax].Occupancy);
                  //  pictureBoxSLAMBest.Refresh();
                  
                    // Draw the worst map
                    pictureBoxSLAMWorst.Image =
                        ProbabilityUtils.DrawLogOddsDistribution(pictureBoxSLAMWorst.Image, maps[indexOfWorst].Occupancy);
                    pictureBoxSLAMWorst.Refresh();
                    

                    // prepare to draw the map of particles
                    double[,] poses = new double[_robot.GlobalMap.Width, _robot.GlobalMap.Height];
                    foreach (Pose2D state in states)
                    {
                        int x = (int) Math.Round(state.X/_robot.GlobalMap.Scale);
                        int y = (int) Math.Round(state.Y/_robot.GlobalMap.Scale);
                        if (x > 0 && y > 0 && x < _robot.GlobalMap.Width && y < _robot.GlobalMap.Height)
                        {
                            poses[x, y] = 1;
                        }
                    }

                    // = //(Image) tmpBest.Clone();
                    ProbabilityUtils.DrawParticles(ref tmpBest, poses);
                     pictureBoxSLAMBest.Image= (Image) tmpBest;
                    pictureBoxSLAMBest.Refresh();

                    if (checkBoxSaveFramesToDisk.Checked)
                    {
                        pictureBoxSLAMBest.Image.Save("frame_" + step.ToString("0000") + ".png",
                                                      System.Drawing.Imaging.ImageFormat.Png);
                    }
                    // draw map of all particles
                    //pictureBoxSLAMAllParticles.Image =
                     //   ProbabilityUtils.DrawDistribution(pictureBoxSLAMAllParticles.Image, poses, new int[0], true );
                   // pictureBoxSLAMAllParticles.Refresh();

                    // update stats image
                    pictureBoxSLAMEffectiveWeight.Image =
                        new Bitmap(pictureBoxSLAMEffectiveWeight.Width, pictureBoxSLAMEffectiveWeight.Height);
                    // draw duplicates (red)
                    pictureBoxSLAMEffectiveWeight.Image =
                        ProbabilityUtils.DrawDistribution(pictureBoxSLAMEffectiveWeight.Image, resamplingHistory, false,
                                                    Color.Red, 1);
                    // draw rms (green)
                    pictureBoxSLAMEffectiveWeight.Image =
                        ProbabilityUtils.DrawDistribution(pictureBoxSLAMEffectiveWeight.Image, rmsErrorHistory, false,
                                                    Color.Green);
                    // draw effective weight (blue)
                    pictureBoxSLAMEffectiveWeight.Image =
                        ProbabilityUtils.DrawDistribution(pictureBoxSLAMEffectiveWeight.Image, effectiveWeightHistory, true,
                                                    Color.Blue);
                    pictureBoxSLAMEffectiveWeight.Refresh();
                }
            }

                #endregion

            }
            System.GC.Collect();
        }

        private void buttonSlimSLAM_Click(object sender, EventArgs e)
        {
            int numParticles = int.Parse(textBoxSLAMParticles.Text);
            int numSteps = int.Parse(textBoxSLAMSteps.Text);
            pictureBoxSLAMBest.Image = new Bitmap(pictureBoxSLAMBest.Width, pictureBoxSLAMBest.Height);

            if (_gridSlam == null)
            {
                // note _robot needs to have been defined
                _gridSlam = new GridSlam(_robot, numParticles);
            }
            
            for (int i = 0; i < numSteps; i++)
            {
                _robot.Move(_robot.Velocity);
                _gridSlam.DoStep(_robot.Velocity, _robot.GetDistanceReadings(_robot.GlobalMap, _robot.Pose2DCurrent, 10));
                
                pictureBoxSLAMBest.Image = _gridSlam.GetBestImage();
                pictureBoxSLAMBest.Refresh();

                pictureBoxSLAMReal.Image = _gridSlam.GetLocalImage();
                pictureBoxSLAMReal.Refresh();
            }
        }

        #endregion

        #region ROBOT
        private void buttonRobotSave_Click(object sender, EventArgs e)
        {
            int robotWidth = int.Parse(textBoxRobotWidth.Text);
            int robotLength = int.Parse(textBoxRobotLength.Text);
            double maxSpeed = double.Parse(textBoxRobotMaxSpeed.Text, CultureInfo.InvariantCulture);
            double maxAccel = double.Parse(textBoxRobotMaxAccel.Text, CultureInfo.InvariantCulture);
            double maxTurn = double.Parse(textBoxRobotMaxTurn.Text, CultureInfo.InvariantCulture);
            double maxTurnAccel = double.Parse(textBoxRobotMaxTurnAccel.Text, CultureInfo.InvariantCulture);

            Map globalMap = new Map(pictureBoxGlobalMap.Width, pictureBoxGlobalMap.Height, 0.6, 10);
            globalMap.LoadImage(pictureBoxGlobalMap.Image);

            _robot = new Robot(new Pose2D(0, 0, 0), robotWidth, robotLength, globalMap);
            _robot.SetMotionParameters(maxSpeed, maxTurn, maxAccel, maxTurnAccel);

            double obstacleClearance = double.Parse(textBoxRobotObstacleClearance.Text, CultureInfo.InvariantCulture);
            double lookAhead = double.Parse(textBoxLookAhead.Text, CultureInfo.InvariantCulture);
            double angleStep = double.Parse(textBoxRobotObstacleAvoidanceAngleStep.Text, CultureInfo.InvariantCulture);
            _robot.ObstacleClearance = obstacleClearance;
            _robot.ObstacleLookAhead = lookAhead;
            _robot.ObstacleAngleStep = angleStep;


            //DataSet ds  = new DataSet();
           textBoxSensorsNum_Leave(null,new EventArgs());
           
              IAvoidObstacles currentNavigator = (IAvoidObstacles) _polarObstacleDensityMap;
            _robot.Navigator = currentNavigator;
            _robot.MotionModel = _motionModel;
            /*
             *  IConeModel currentConeModel = (comboBoxConeModel.SelectedIndex == 0) ? (IConeModel)_coneModel : (IConeModel)_coneModel4Gaussians;
         
            _robot.AddDistanceSensor(new DistanceSensor(0, 0, -2.75, _beamModel, currentConeModel));
            _robot.AddDistanceSensor(new DistanceSensor(0, 0, -2.5, _beamModel, currentConeModel));
            _robot.AddDistanceSensor(new DistanceSensor(0, 0, -2.25, _beamModel, currentConeModel));
            _robot.AddDistanceSensor(new DistanceSensor(0, 0, -2, _beamModel, currentConeModel));
            _robot.AddDistanceSensor(new DistanceSensor(0, 0, -1.75, _beamModel, currentConeModel));
            _robot.AddDistanceSensor(new DistanceSensor(0, 0, -1.5, _beamModel, currentConeModel));
            _robot.AddDistanceSensor(new DistanceSensor(0, 0, -1.25, _beamModel, currentConeModel));
            _robot.AddDistanceSensor(new DistanceSensor(0, 0, -1, _beamModel, currentConeModel));
            _robot.AddDistanceSensor(new DistanceSensor(0, 0, -0.75, _beamModel, currentConeModel));
            _robot.AddDistanceSensor(new DistanceSensor(0, 0, -0.5, _beamModel, currentConeModel));
            _robot.AddDistanceSensor(new DistanceSensor(0, 0, -0.25, _beamModel, currentConeModel));
            _robot.AddDistanceSensor(new DistanceSensor(0, 0, 0, _beamModel, currentConeModel));
            _robot.AddDistanceSensor(new DistanceSensor(0, 0, 0.25, _beamModel, currentConeModel));
            _robot.AddDistanceSensor(new DistanceSensor(0, 0, 0.5, _beamModel, currentConeModel));
            _robot.AddDistanceSensor(new DistanceSensor(0, 0, 0.75, _beamModel, currentConeModel));
            _robot.AddDistanceSensor(new DistanceSensor(0, 0, 1, _beamModel, currentConeModel));
            _robot.AddDistanceSensor(new DistanceSensor(0, 0, 1.25, _beamModel, currentConeModel));
            _robot.AddDistanceSensor(new DistanceSensor(0, 0, 1.5, _beamModel, currentConeModel));
            _robot.AddDistanceSensor(new DistanceSensor(0, 0, 1.75, _beamModel, currentConeModel));
            _robot.AddDistanceSensor(new DistanceSensor(0, 0, 2, _beamModel, currentConeModel));
            _robot.AddDistanceSensor(new DistanceSensor(0, 0, 2.25, _beamModel, currentConeModel));
            _robot.AddDistanceSensor(new DistanceSensor(0, 0, 2.5, _beamModel, currentConeModel));
            _robot.AddDistanceSensor(new DistanceSensor(0, 0, 2.75, _beamModel, currentConeModel));
            _robot.AddDistanceSensor(new DistanceSensor(0, 0, 3.14, _beamModel, currentConeModel));
            */

           
        }

        public DataTable GetSensorsAsDataSet(List<DistanceSensor> distanceSensors)
        {
            DataTable sensors = new DataTable("Sensors");
            sensors.Columns.Add("x", typeof (double));
            sensors.Columns.Add("y", typeof(double));
            sensors.Columns.Add("theta", typeof(double));
            sensors.Columns.Add("maxRange", typeof(double));
            sensors.Columns.Add("hitVariance", typeof(double));
            sensors.Columns.Add("shortDecay", typeof(double));
            sensors.Columns.Add("coneWidth", typeof(double));
            sensors.Columns.Add("obstacleSize" , typeof(double));

            foreach (DistanceSensor sensor in distanceSensors)
            {
                DataRow sensorRow = sensors.NewRow();
                sensorRow["x"] = sensor.X;
                sensorRow["y"] = sensor.Y;
                sensorRow["theta"] = sensor.Theta;
                sensorRow["maxRange"] = sensor.Beam.MaxRange;
                sensorRow["hitVariance"] = sensor.Beam.BeamHitVariance;
                sensorRow["shortDecay"] = sensor.Beam.BeamShortDecay;
                sensorRow["coneWidth"] = sensor.Cone.ConeWidth;
                sensorRow["obstacleSize"] = sensor.Cone.ObstacleSize;

                sensors.Rows.Add(sensorRow);
            }

            return sensors;            
        }

        /// <summary>
        /// Save Robot avoidance details
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void buttonObstacleAvoidanceSave_Click(object sender, EventArgs e)
        {
            double obstacleClearance = double.Parse(textBoxRobotObstacleClearance.Text, CultureInfo.InvariantCulture);
            double lookAhead = double.Parse(textBoxLookAhead.Text, CultureInfo.InvariantCulture);
            double angleStep = double.Parse(textBoxRobotObstacleAvoidanceAngleStep.Text, CultureInfo.InvariantCulture);

            _robot.ObstacleClearance = obstacleClearance;
            _robot.ObstacleLookAhead = lookAhead;
            _robot.ObstacleAngleStep = angleStep;
        }

        private void buttonRobotTestContraints_Click(object sender, EventArgs e)
        {
            Image img = new Bitmap(pictureBoxRobotTest.Width, pictureBoxRobotTest.Height);
            pictureBoxRobotTest.Image = img;
            Graphics g = Graphics.FromImage(img);

            // test robot params
            int xOffset = 50;
            int yOffset = pictureBoxRobotTest.Height/2;

            int obstacleDistance = int.Parse(textBoxRobotObstacleDistance.Text);
            int obstacleSize = int.Parse(textBoxRobotObstacleSize.Text);

            // PREPARE THE ROBOT
            Pose2D state =
                new Pose2D(xOffset - _robot.Length/2, yOffset - _robot.Width/2, 0);
            _robot.Pose2DCurrent = state;

            Map m = new Map(pictureBoxRobotTest.Width, pictureBoxRobotTest.Height, 0.6, 1);
            int obstacleLeft = (int) Math.Round(state.X) + obstacleDistance;
            int obstacleTop = (int) Math.Round(state.Y) - obstacleSize/2;
            int obstacleRight = obstacleLeft + obstacleSize;
            int obstacleBottom = obstacleTop + obstacleSize;

            // ADD THE OBSTACLE TO THE MAP
            for (int x = obstacleLeft; x < obstacleRight; x++)
            {
                for (int y = obstacleTop; y < obstacleBottom; y++)
                {
                    m.Occupancy[x, y] = ProbabilityUtils.ProbabilityToLogOdds((0.8));
                }
            }
            _robot.GlobalMap = m;
            DistanceReading[] readings = _robot.GetDistanceReadings(_robot.GlobalMap, _robot.Pose2DCurrent, 0);
            _robot.GlobalMap.AddReadings(_robot.Pose2DCurrent, readings);
            // DRAW THE MAP
           // double[,] dist = ProbabilityUtils.LogOddsToProbability(_robot.GlobalMap.Occupancy);
            pictureBoxRobotTest.Image = ProbabilityUtils.DrawLogOddsDistribution(pictureBoxRobotTest.Image, _robot.GlobalMap.Occupancy);
            pictureBoxRobotTest.Refresh();

           // DifferentialDriveState testState = (DifferentialDriveState)_robot.State.Clone();
            Velocity vel = new Velocity(_robot.MaxSpeed, 0);

            //  double angleStep = double.Parse(textBoxRobotObstacleAvoidanceAngleStep.Text, CultureInfo.InvariantCulture);

           // bool success = false;
            
            // *** GET SAFE VEL
            Point[] points = new Point[0];
            vel = _robot.GetSafeVelocity(state, vel, out points);
           
           // Console.Out.WriteLine("Success at " + vel.ToString());
          
            Rectangle robotRectangle = new Rectangle((int)Math.Round(state.X - _robot.Length / 2), (int)Math.Round(state.Y - _robot.Width / 2), (int)Math.Round(_robot.Length), (int)Math.Round(_robot.Width));
            g.DrawRectangle(Pens.Blue, robotRectangle);
            if (points.Length > 1)
            {
                if (vel.Translation > 0)
                {
                    labelRobotResultVelocity.Text = "Resultant velocity: " + vel.ToString();
                    g.DrawLines(Pens.Blue, points);
               
                }
                else
                {
                    labelRobotResultVelocity.Text = "No safe path found.";
                    g.DrawLines(Pens.Red, points);
                }
            }
            g.Save();
            g.Dispose();
            pictureBoxRobotTest.Refresh();
        }

        private void comboBoxConeModel_SelectedIndexChanged(object sender, EventArgs e)
        {
            // save all the sensor with a different cone model
            foreach (DistanceSensor ds in _robot.DistanceSensors)
            {
                if (comboBoxConeModel.SelectedIndex == 0)
                {
                    ds.Cone = _coneModel;
                }
                else
                {
                    ds.Cone = _coneModel4Gaussians;
                }
            }
        }

        private void comboBoxNavigator_SelectedIndexChanged(object sender, EventArgs e)
        {
            _robot.Navigator = _polarObstacleDensityMap;
            /*
            // save a new navigator
            if (comboBoxNavigator.SelectedIndex == 0)
            {
                
            }
            else 
            {
                
            }
             */
        }
        #endregion        

        #region Sonar Model Tab

        private void buttonTestBeamModel_Click(object sender, EventArgs e)
        {
            DrawBeamModel();
           // DrawConeModel();
        }

        private void buttonConeModelSave_Click(object sender, EventArgs e)
        {
            _coneModel = new ConeModel1Gaussian(double.Parse(textBoxConeMaxRange.Text, CultureInfo.InvariantCulture),
                double.Parse(textBoxConeBeamWidth.Text, CultureInfo.InvariantCulture),
                double.Parse(textBoxConeVariance.Text, CultureInfo.InvariantCulture),
                double.Parse(textBoxConeObstacleSize.Text, CultureInfo.InvariantCulture),
                double.Parse(textBoxConeTruthProb.Text, CultureInfo.InvariantCulture),
                double.Parse(textBoxConeFalseProb.Text, CultureInfo.InvariantCulture));
            DrawConeModel();
        }

        private void DrawConeModel()
        {
            pictureBoxConeModel.Image = new Bitmap(pictureBoxConeModel.Width, pictureBoxConeModel.Height);
            double[,] map = new double[pictureBoxConeModel.Image.Width, pictureBoxConeModel.Image.Height];
            Rectangle updateRect;
            map = _coneModel.UpdateMap(map, new Pose2D(10, 50, 0), new double[] { double.Parse(textBoxConeModelDistance.Text, CultureInfo.InvariantCulture), 0 }, 1, out updateRect);
            //double[,] probMap = ProbabilityUtils.LogOddsToProbability(map);
            ProbabilityUtils.DrawLogOddsDistribution(pictureBoxConeModel.Image, map);//  updateRect);
            pictureBoxConeModel.Refresh();
        }

        private void buttonBeamModelSave_Click(object sender, EventArgs e)
        {
            _beamModel = new BeamModel(double.Parse(textBoxBeamHitVariance.Text, CultureInfo.InvariantCulture),
             double.Parse(textBoxBeamShortDecay.Text, CultureInfo.InvariantCulture),
             double.Parse(textBoxBeamMaxRange.Text, CultureInfo.InvariantCulture),
             double.Parse(textBoxBeamMixHit.Text, CultureInfo.InvariantCulture),
             double.Parse(textBoxBeamMixShort.Text, CultureInfo.InvariantCulture),
             double.Parse(textBoxBeamMixRandom.Text, CultureInfo.InvariantCulture),
             double.Parse(textBoxBeamMixMax.Text, CultureInfo.InvariantCulture));

            _beamModel.Prepare();

            DrawBeamModel();
        }

        private void DrawBeamModel()
        {
            pictureBoxBeamModel.Image = new Bitmap(pictureBoxBeamModel.Width, pictureBoxBeamModel.Height);
            double[] distribution = _beamModel.GetDistribution(double.Parse(textBoxBeamModelDistance.Text, CultureInfo.InvariantCulture));
            pictureBoxBeamModel.Image = ProbabilityUtils.DrawDistribution(pictureBoxBeamModel.Image, distribution,true, Color.Blue);
            pictureBoxBeamModel.Refresh();
        }
        #endregion

        #region ConeModel4Gaussians
        private void buttonConeModel4DTestHit_Click(object sender, EventArgs e)
        {
            if (_coneModel4Gaussians == null)
            {
                buttonConeModel4DSave_Click(null, new EventArgs());
            }
           
                pictureBoxSonarModel2.Image = new Bitmap(pictureBoxSonarModel2.Width, pictureBoxSonarModel2.Height);
                _robot.GlobalMap = new Map(pictureBoxSonarModel2.Width, pictureBoxSonarModel2.Height, 0.6, 1);


                double x = double.Parse(textBoxConeModel4DTestX.Text, CultureInfo.InvariantCulture);
                double y = double.Parse(textBoxConeModel4DTestY.Text, CultureInfo.InvariantCulture);
                double angle = double.Parse(textBoxConeModel4DTestAngle.Text, CultureInfo.InvariantCulture);
                double range = double.Parse(textBoxConeModel4DTestRange.Text, CultureInfo.InvariantCulture);
            //double [,] logOdds = new double[pictureBoxSonarModel2.Width,pictureBoxSonarModel2.Height];
            DistanceReading distanceReading = new DistanceReading(x, y, angle, range, _beamModel, _coneModel4Gaussians);
            _robot.GlobalMap.Occupancy =_coneModel4Gaussians.UpdateMap(_robot.GlobalMap.Occupancy, distanceReading, 1);
           // double[] maxmintotLog = ProbabilityUtils.GetMaxMinTotal(_robot.GlobalMap.Occupancy);
            //double[,] prob = ProbabilityUtils.LogOddsToProbability();
           // double[] maxmintot = ProbabilityUtils.GetMaxMinTotal(prob);
            pictureBoxSonarModel2.Image =
                ProbabilityUtils.DrawLogOddsDistribution(pictureBoxSonarModel2.Image, _robot.GlobalMap.Occupancy);
           // Console.Out.WriteLine(string.Format("Log: {0},{1},{2}", maxmintotLog[0], maxmintotLog[1], maxmintotLog[2]));
           // Console.Out.WriteLine(string.Format("Pro: {0},{1},{2}", maxmintot[0], maxmintot[1], maxmintot[2]));
        }

        private void buttonConeModel4DSave_Click(object sender, EventArgs e)
        {
            _coneModel4Gaussians = new ConeModel4Gaussians(
                double.Parse(textBoxConeModel4DMaxRange.Text, CultureInfo.InvariantCulture),
                double.Parse(textBoxConeModel4DConeWidth.Text, CultureInfo.InvariantCulture),
                double.Parse(textBoxConeModel4DObstacleSize.Text, CultureInfo.InvariantCulture));
        }

        private void buttonTestWall_Click(object sender, EventArgs e)
        {
            if (_coneModel4Gaussians == null)
            {
                buttonConeModel4DSave_Click(null, new EventArgs());
            }
            
                pictureBoxSonarModel2.Image = new Bitmap(pictureBoxSonarModel2.Width, pictureBoxSonarModel2.Height);
                _robot.GlobalMap = new Map(pictureBoxSonarModel2.Width, pictureBoxSonarModel2.Height, 0.6, 1);
            

            //double [,] logOdds = new double[pictureBoxSonarModel2.Width,pictureBoxSonarModel2.Height];
            for (int i = 10; i < pictureBoxSonarModel2.Image.Height; i += 20)
            {
                DistanceReading distanceReading =
                    new DistanceReading(10, i, 0, double.Parse(textBoxConeModel4DTestRange.Text, CultureInfo.InvariantCulture), _beamModel,
                                        _coneModel4Gaussians);
                
                   _robot.GlobalMap.Occupancy = _coneModel4Gaussians.UpdateMap( _robot.GlobalMap.Occupancy, distanceReading, 1);


               // double[,] prob = ProbabilityUtils.LogOddsToProbability();
                pictureBoxSonarModel2.Image =
                    ProbabilityUtils.DrawLogOddsDistribution(pictureBoxSonarModel2.Image, _robot.GlobalMap.Occupancy);
                pictureBoxSonarModel2.Refresh();
            }
        }

        private void buttonTestBall_Click(object sender, EventArgs e)
        {
            if (_coneModel4Gaussians == null)
            {
                buttonConeModel4DSave_Click(null, new EventArgs());
            }
            
                pictureBoxSonarModel2.Image = new Bitmap(pictureBoxSonarModel2.Width, pictureBoxSonarModel2.Height);
                _robot.GlobalMap = new Map(pictureBoxSonarModel2.Width, pictureBoxSonarModel2.Height, 0.6, 1);
            

            // moves around in a circle, pointing at the center
            int ox = pictureBoxSonarModel2.Image.Width/2;
            int oy = pictureBoxSonarModel2.Image.Height/2;
            double range = double.Parse(textBoxConeModel4DTestRange.Text, CultureInfo.InvariantCulture);

            for (double i = 0; i < Math.PI *2; i += 0.1)
            {
                int x = ox + (int)Math.Round(Math.Cos(i) * range);
                int y = oy + (int)Math.Round(Math.Sin(i) * range);
                double angle = i -Math.PI;
           
                DistanceReading distanceReading =
                    new DistanceReading(x,y, angle, range, _beamModel,
                                        _coneModel4Gaussians);
                
                   _robot.GlobalMap.Occupancy = _coneModel4Gaussians.UpdateMap( _robot.GlobalMap.Occupancy, distanceReading, 1);


               // double[,] prob = ProbabilityUtils.LogOddsToProbability();
                pictureBoxSonarModel2.Image =
                    ProbabilityUtils.DrawLogOddsDistribution(pictureBoxSonarModel2.Image, _robot.GlobalMap.Occupancy);
                pictureBoxSonarModel2.Refresh();
            }
        }
        #endregion

        #region VFH
        private void buttonCreateVFH_Click(object sender, EventArgs e)
        {
            pictureBoxVFH.Image = new Bitmap(pictureBoxVFH.Width, pictureBoxVFH.Height);
            pictureBoxVFHSmooth.Image = new Bitmap(pictureBoxVFHSmooth.Width, pictureBoxVFHSmooth.Height);
            pictureBoxVFHResults.Image = new Bitmap(pictureBoxVFHResults.Width, pictureBoxVFHResults.Height);

            // create and set params
           
            _polarObstacleDensityMap.PopulateFromMap(_robot.GlobalMap, _robot.Pose2DCurrent);
           
            // draw the raw distribution
            pictureBoxVFH.Image = _polarObstacleDensityMap.DrawToImage(pictureBoxVFH.Image);
            pictureBoxVFH.Refresh();

            // truncate, smooth and find valleys
            _polarObstacleDensityMap.PrepareValleys();
 
            // draw the smoothed version
            pictureBoxVFHSmooth.Image = _polarObstacleDensityMap.DrawToImage(pictureBoxVFHSmooth.Image);
            pictureBoxVFHSmooth.Refresh();
           
            // draw valleys and direction to the map
            Velocity desiredVel = new Velocity(_robot.Velocity.Translation,_robot.Pose2DCurrent.Theta);
            pictureBoxVFHResults.Image = _polarObstacleDensityMap.DrawValleysAndDirections(pictureBoxVFHResults.Image, desiredVel);
            pictureBoxVFHResults.Refresh();
            foreach (int[] ints in _polarObstacleDensityMap.Valleys)
            {
                Console.Out.WriteLine("Valley: " + ints[0] + " to " + ints[1]);
            }

            Console.Out.WriteLine("resultant velocity is " + _polarObstacleDensityMap.FindSafeVelocity(_robot.Velocity) + " for " + _robot.Velocity);
            Console.Out.WriteLine("Robot is : " + _robot.Pose2DCurrent.ToString() + " vel: " + _robot.Velocity.ToString());
        }



        /// <summary>
        /// Save the Vector Field Histogram Model
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void buttonVFHSave_Click(object sender, EventArgs e)
        {
            _polarObstacleDensityMap = new PolarObstacleDensityMap(int.Parse(textBoxVFHNumSegments.Text));

            _polarObstacleDensityMap.SetVFHParams(double.Parse(textBoxVFHTruncateAt.Text, CultureInfo.InvariantCulture),
               int.Parse(textBoxVFHSmoothingRange.Text),
               double.Parse(textBoxVFHValleyThreashold.Text, CultureInfo.InvariantCulture),
               int.Parse(textBoxVFHLargeValley.Text)
               , double.Parse(textBoxVFHRange.Text, CultureInfo.InvariantCulture));

            if (_robot != null)
            {
                _robot.Navigator = _polarObstacleDensityMap;
            }
        }
        #endregion

        private void buttonSonarTestLearning_Click(object sender, EventArgs e)
        {
            _robot.DistanceSensors[0].Beam.TestLearning();
        }

        private void LoadDefaultMap()
        {
            pictureBoxGlobalMap.Image = Image.FromFile("maps/defaultMap.jpg");
        }

        private void openMapToolStripMenuItem_Click(object sender, EventArgs e)
        {
            //openFileDialog1.Filter = "*.jpg|*.png";
            System.Windows.Forms.DialogResult result = openFileDialog1.ShowDialog();
            if (result == DialogResult.OK && openFileDialog1.CheckFileExists)
            {
                Image img = Bitmap.FromStream(openFileDialog1.OpenFile());
                openFileDialog1.Dispose();
               // _robot.GlobalMap.LoadImage(img);
                pictureBoxGlobalMap.Image = img;
               // pictureBoxSimilarity.Image = (Bitmap)img.Clone();
                pictureBoxGlobalMap.Refresh();
               // pictureBoxSimilarity.Refresh();
            }
        }

        private void aboutToolStripMenuItem_Click(object sender, EventArgs e)
        {
            About about = new About();
            about.ShowDialog();
            //about.Activate();

        }

        private void buttonUpdateSensorGrid_Click(object sender, EventArgs e)
        {
            
            DataTable sensors = GetSensorsAsDataSet(_robot.DistanceSensors);
            dataGridViewSensors.DataSource = sensors;
            dataGridViewSensors.Show();

        }

        private void dataGridViewSensors_RowsRemoved(object sender, DataGridViewRowsRemovedEventArgs e)
        {
            /*if (e.RowIndex != null && e.RowIndex >= 0 && e.RowIndex < _robot.DistanceSensors.Count)
            {
                _robot.DistanceSensors.RemoveAt(e.RowIndex);
                button2_Click(null, new EventArgs());
            }*/
        }

        private void buttonClear_Click(object sender, EventArgs e)
        {
            Image i2 = new Bitmap(pictureBox2.Width, pictureBox2.Height);
            pictureBox2.Image = i2;
            _pf = null;
            _samples = null;
        }

        private void buttonTestBeamModel_Click_1(object sender, EventArgs e)
        {
            DrawConeModel();
        }

        private void quitToolStripMenuItem_Click(object sender, EventArgs e)
        {
            Application.Exit();
        }

        private void radioButtonSensorsSonars_Click(object sender, EventArgs e)
        {
          
        }

        private void radioButtonSensorsSonars_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButtonSensorsSonars.Checked == true)
            {
                // create beam model
                _beamModel = new BeamModel(300,0.01,200,0.6,0.2,0.2,0);
                textBoxBeamHitVariance.Text = "300";
                textBoxBeamShortDecay.Text = "0.01";
                textBoxBeamMaxRange.Text = "200";
                textBoxBeamMixHit.Text = "0.6";
                textBoxBeamMixShort.Text = "0.2";
                textBoxBeamMixRandom.Text = "0.2";
                textBoxBeamMixMax.Text = "0";
                DrawBeamModel();

                // create cone model
               
                _coneModel = new ConeModel1Gaussian(200,0.5,0.004,10,0.3,0.1);
                textBoxConeMaxRange.Text = "200";
                textBoxConeBeamWidth.Text = "0.5";
                textBoxConeVariance.Text = "0.004";
                textBoxConeObstacleSize.Text = "10";
                textBoxConeTruthProb.Text = "0.3";
                textBoxConeFalseProb.Text = "0.1";
                DrawConeModel();

                _coneModel4Gaussians = new ConeModel4Gaussians(200,0.5,10);
                textBoxConeModel4DMaxRange.Text = "200";
                textBoxConeModel4DConeWidth.Text = "0.5";
                textBoxConeModel4DObstacleSize.Text = "10";
                buttonConeModel4DTestHit_Click(null, new EventArgs());
                IConeModel currentConeModel = (comboBoxConeModel.SelectedIndex == 0) ? (IConeModel)_coneModel : (IConeModel)_coneModel4Gaussians;
         
                // create n sensors
                int numSensors;
                if (int.TryParse(textBoxSensorsNum.Text, out numSensors))
                {
                    double sensorSpacing = Math.PI*2.0/numSensors;
                    _robot.DistanceSensors.Clear();

                    for (int i = 0; i < numSensors; i++)
                    {
                        _robot.AddDistanceSensor(new DistanceSensor(0, 0, (sensorSpacing * i), _beamModel, currentConeModel));
                    }
                }
            }
            buttonUpdateSensorGrid_Click(null, new EventArgs());
          //  buttonTestBeamModel_Click(null, new EventArgs());
          //  buttonRobotTestContraints_Click(null,new EventArgs());
           // buttonConeModelSave_Click(null,new EventArgs());
           // buttonConeModel4DTestHit_Click(null,new EventArgs());
        }

    

        private void textBoxSensorsNum_Leave(object sender, EventArgs e)
        {
            if (radioButtonSensorsSonars.Checked == true)
            {
                radioButtonSensorsSonars_CheckedChanged(null, new EventArgs());
            } 
            else if (radioButtonSensorsIR.Checked == true)
            {
                radioButtonSensorsIR_CheckedChanged(null,new EventArgs());
            }
            else if (radioButtonSensorsLaser.Checked == true)
            {
                radioButtonSensorsLaser_CheckedChanged(null, new EventArgs());
            }
            else if (radioButtonSensorsCustom.Checked == true)
            {
                radioButtonSensorsCustom_CheckedChanged(null, new EventArgs());
            }
        }

        private void radioButtonSensorsIR_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButtonSensorsIR.Checked == true)
            {
                // create beam model
                _beamModel = new BeamModel(50, 0.01, 140, 0.6, 0.2, 0.2, 0);
                textBoxBeamHitVariance.Text = "50";
                textBoxBeamShortDecay.Text = "0.01";
                textBoxBeamMaxRange.Text = "140";
                textBoxBeamMixHit.Text = "0.6";
                textBoxBeamMixShort.Text = "0.2";
                textBoxBeamMixRandom.Text = "0.2";
                textBoxBeamMixMax.Text = "0";
                DrawBeamModel();
                // create cone model

                _coneModel = new ConeModel1Gaussian(140, 0.03, 0.004, 10, 0.3, 0.1);
                textBoxConeMaxRange.Text = "140";
                textBoxConeBeamWidth.Text = "0.03";
                textBoxConeVariance.Text = "0.004";
                textBoxConeObstacleSize.Text = "10";
                textBoxConeTruthProb.Text = "0.3";
                textBoxConeFalseProb.Text = "0.1";
                DrawConeModel();

                _coneModel4Gaussians = new ConeModel4Gaussians(140, 0.03, 10);
                textBoxConeModel4DMaxRange.Text = "140";
                textBoxConeModel4DConeWidth.Text = "0.03";
                textBoxConeModel4DObstacleSize.Text = "10";
                buttonConeModel4DTestHit_Click(null, new EventArgs());
                IConeModel currentConeModel = (comboBoxConeModel.SelectedIndex == 0) ? (IConeModel)_coneModel : (IConeModel)_coneModel4Gaussians;

                // create n sensors
                int numSensors;
                if (int.TryParse(textBoxSensorsNum.Text, out numSensors))
                {
                    double sensorSpacing = Math.PI * 2.0 / numSensors;
                    _robot.DistanceSensors.Clear();

                    for (int i = 0; i < numSensors; i++)
                    {
                        _robot.AddDistanceSensor(new DistanceSensor(0, 0, (sensorSpacing * i), _beamModel, currentConeModel));
                    }
                }
            }
            buttonUpdateSensorGrid_Click(null, new EventArgs());
        }

        private void radioButtonSensorsLaser_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButtonSensorsLaser.Checked == true)
            {
                // create beam model
                _beamModel = new BeamModel(500, 0.01, 600, 0.6, 0.2, 0.2, 0);
                textBoxBeamHitVariance.Text = "500";
                textBoxBeamShortDecay.Text = "0.01";
                textBoxBeamMaxRange.Text = "600";
                textBoxBeamMixHit.Text = "0.6";
                textBoxBeamMixShort.Text = "0.2";
                textBoxBeamMixRandom.Text = "0.2";
                textBoxBeamMixMax.Text = "0";
                DrawBeamModel();
                // create cone model

                _coneModel = new ConeModel1Gaussian(600, 0.03, 0.04, 10, 0.3, 0.1);
                textBoxConeMaxRange.Text = "600";
                textBoxConeBeamWidth.Text = "0.03";
                textBoxConeVariance.Text = "0.004";
                textBoxConeObstacleSize.Text = "10";
                textBoxConeTruthProb.Text = "0.3";
                textBoxConeFalseProb.Text = "0.1";
                DrawConeModel();

                _coneModel4Gaussians = new ConeModel4Gaussians(600, 0.03, 10);
                textBoxConeModel4DMaxRange.Text = "600";
                textBoxConeModel4DConeWidth.Text = "0.03";
                textBoxConeModel4DObstacleSize.Text = "10";
                buttonConeModel4DTestHit_Click(null, new EventArgs());

                IConeModel currentConeModel = (comboBoxConeModel.SelectedIndex == 0) ? (IConeModel)_coneModel : (IConeModel)_coneModel4Gaussians;

                // create n sensors
                int numSensors;
                if (int.TryParse(textBoxSensorsNum.Text, out numSensors))
                {
                    double sensorSpacing = Math.PI * 2.0 / numSensors;
                    _robot.DistanceSensors.Clear();

                    for (int i = 0; i < numSensors; i++)
                    {
                        _robot.AddDistanceSensor(new DistanceSensor(0, 0, (sensorSpacing * i), _beamModel, currentConeModel));
                    }
                }
            }
            buttonUpdateSensorGrid_Click(null, new EventArgs());
        }



        private void radioButtonSensorsCustom_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButtonSensorsCustom.Checked == true)
            {
                IConeModel currentConeModel = (comboBoxConeModel.SelectedIndex == 0) ? (IConeModel)_coneModel : (IConeModel)_coneModel4Gaussians;

                // create n sensors
                int numSensors;
                if (int.TryParse(textBoxSensorsNum.Text, out numSensors))
                {
                    double sensorSpacing = Math.PI * 2.0 / numSensors;
                    _robot.DistanceSensors.Clear();

                    for (int i = 0; i < numSensors; i++)
                    {
                        _robot.AddDistanceSensor(new DistanceSensor(0, 0, (sensorSpacing * i), _beamModel, currentConeModel));
                    }
                }
            }
            buttonUpdateSensorGrid_Click(null, new EventArgs());
        }

        private void openLogFileToolStripMenuItem_Click(object sender, EventArgs e)
        {
            LogFileImport logFileImport = new LogFileImport(this);
            logFileImport.Show();
        }

        private void button3_Click(object sender, EventArgs e)
        {
            _gridSlam = null;
        }

        private void tabPage1_Click(object sender, EventArgs e)
        {

        }

        private void tabPageMotionModel_Click(object sender, EventArgs e)
        {

        }

        private void groupBox23_Enter(object sender, EventArgs e)
        {

        }

        private void label129_Click(object sender, EventArgs e)
        {

        }

        private void label123_Click(object sender, EventArgs e)
        {

        }

        private void buttonOdometryModelSave_Click(object sender, EventArgs e)
        {
            _odometryMotionModel = new OdometryMotionModel(
                double.Parse(textBoxOdometryModelVErr1.Text,CultureInfo.InvariantCulture),
                double.Parse(textBoxOdometryModelVErr2.Text, CultureInfo.InvariantCulture),
                double.Parse(textBoxOdometryModelRErr1.Text, CultureInfo.InvariantCulture),
                double.Parse(textBoxOdometryModelRErr2.Text, CultureInfo.InvariantCulture));

        }

        private void buttonOdometryModelStep_Click(object sender, EventArgs e)
        {
            if (_odometryMotionModel == null)
            {
                buttonOdometryModelSave_Click(null, new EventArgs());
            }

            if (_odometryModelState == null)
            {
                buttonOdometryModelClear_Click(null, new EventArgs());
            }

            Pose2D finishState = new Pose2D(
               _odometryModelState.X + double.Parse(textBoxOdometryModelDX.Text, CultureInfo.InvariantCulture),
               _odometryModelState.Y + double.Parse(textBoxOdometryModelDY.Text, CultureInfo.InvariantCulture),
               _odometryModelState.Theta + double.Parse(textBoxOdometryModelDTheta.Text, CultureInfo.InvariantCulture));

            _odometryMotionModel.PrepareForSampling(finishState,_odometryModelState);

            // create start particles
            int numSamples = int.Parse(textBoxParticles.Text, CultureInfo.InvariantCulture);

            
           


            double[] weights = new double[numSamples];

            for (int i = 0; i < numSamples; i++)
            {
                
                Pose2D newState = _odometryMotionModel.GetSample(_odometryModelStates[i]);
                if (checkBoxOdometryModelResample.Checked)
                {
                    weights[i] = _odometryMotionModel.GetProbabilityOfSample(newState, _odometryModelStates[i]);
                }
                _odometryModelStates[i] = newState;
            }

            
            if (checkBoxOdometryModelResample.Checked)
            { 
                weights = ProbabilityUtils.Normalize(weights, Normalization.IntegralToOne);
                int[] indexes = ProbabilityUtils.ReSample(weights);

                Pose2D[] newStates = new Pose2D[numSamples];
                int s = 0;
                foreach (int i in indexes)
                {
                    newStates[s++] = _odometryModelStates[i];
                }
                _odometryModelStates = newStates;
            }

            Bitmap tmpImage = (Bitmap)pictureBoxOdometryModel.Image;
            ProbabilityUtils.DrawParticles(ref tmpImage, _odometryModelStates, 1);
            pictureBoxOdometryModel.Image = tmpImage;
            pictureBoxOdometryModel.Refresh();
        }

        private void Form1_Load(object sender, EventArgs e)
        {

        }

        private void buttonOdometryModelClear_Click(object sender, EventArgs e)
        {

            int numSamples = int.Parse(textBoxParticles.Text, CultureInfo.InvariantCulture);

            _odometryModelState = new Pose2D(
                double.Parse(textBoxOdometryModelX.Text, CultureInfo.InvariantCulture),
                double.Parse(textBoxOdometryModelY.Text, CultureInfo.InvariantCulture),
                double.Parse(textBoxOdometryModelTheta.Text, CultureInfo.InvariantCulture));

            _odometryModelStates = new Pose2D[numSamples];
            for (int i = 0; i < numSamples; i++)
            {
                _odometryModelStates[i] = (Pose2D)_odometryModelState.Clone();
            }

            pictureBoxOdometryModel.Image = new Bitmap(pictureBoxOdometryModel.Width, pictureBoxOdometryModel.Height);
            Bitmap tmpImage = (Bitmap)pictureBoxOdometryModel.Image;
            ProbabilityUtils.DrawParticles(ref tmpImage, _odometryModelStates, 1);
            pictureBoxOdometryModel.Image = tmpImage;
            pictureBoxOdometryModel.Refresh();

        }

    }
}